#include "navmesh.h"

#include <cassert>
inline bool inRange(const float *v1, const float *v2, const float r,
                    const float h) {
  const float dx = v2[0] - v1[0];
  const float dy = v2[1] - v1[1];
  const float dz = v2[2] - v1[2];
  return (dx * dx + dz * dz) < r * r && fabsf(dy) < h;
}

static bool getSteerTarget(dtNavMeshQuery *navQuery, const float *startPos,
                           const float *endPos, const float minTargetDist,
                           const dtPolyRef *path, const int pathSize,
                           float *steerPos, unsigned char &steerPosFlag,
                           dtPolyRef &steerPosRef, float *outPoints = 0,
                           int *outPointCount = 0) {
  // Find steer target.
  static const int MAX_STEER_POINTS = 3;
  float steerPath[MAX_STEER_POINTS * 3];
  unsigned char steerPathFlags[MAX_STEER_POINTS];
  dtPolyRef steerPathPolys[MAX_STEER_POINTS];
  int nsteerPath = 0;
  navQuery->findStraightPath(startPos, endPos, path, pathSize, steerPath,
                             steerPathFlags, steerPathPolys, &nsteerPath,
                             MAX_STEER_POINTS);
  if (!nsteerPath) return false;

  if (outPoints && outPointCount) {
    *outPointCount = nsteerPath;
    for (int i = 0; i < nsteerPath; ++i)
      dtVcopy(&outPoints[i * 3], &steerPath[i * 3]);
  }

  // Find vertex far enough to steer to.
  int ns = 0;
  while (ns < nsteerPath) {
    // Stop at Off-Mesh link or when point is further than slop away.
    if ((steerPathFlags[ns] & DT_STRAIGHTPATH_OFFMESH_CONNECTION) ||
        !inRange(&steerPath[ns * 3], startPos, minTargetDist, 1000.0f))
      break;
    ns++;
  }
  // Failed to find good point to steer to.
  if (ns >= nsteerPath) return false;

  dtVcopy(steerPos, &steerPath[ns * 3]);
  steerPos[1] = startPos[1];
  steerPosFlag = steerPathFlags[ns];
  steerPosRef = steerPathPolys[ns];

  return true;
}

static int fixupCorridor(dtPolyRef *path, const int npath, const int maxPath,
                         const dtPolyRef *visited, const int nvisited) {
  int furthestPath = -1;
  int furthestVisited = -1;

  // Find furthest common polygon.
  for (int i = npath - 1; i >= 0; --i) {
    bool found = false;
    for (int j = nvisited - 1; j >= 0; --j) {
      if (path[i] == visited[j]) {
        furthestPath = i;
        furthestVisited = j;
        found = true;
      }
    }
    if (found) break;
  }

  // If no intersection found just return current path.
  if (furthestPath == -1 || furthestVisited == -1) return npath;

  // Concatenate paths.

  // Adjust beginning of the buffer to include the visited.
  const int req = nvisited - furthestVisited;
  const int orig = rcMin(furthestPath + 1, npath);
  int size = rcMax(0, npath - orig);
  if (req + size > maxPath) size = maxPath - req;
  if (size) memmove(path + req, path + orig, size * sizeof(dtPolyRef));

  // Store visited
  for (int i = 0; i < req; ++i) path[i] = visited[(nvisited - 1) - i];

  return req + size;
}

// This function checks if the path has a small U-turn, that is,
// a polygon further in the path is adjacent to the first polygon
// in the path. If that happens, a shortcut is taken.
// This can happen if the target (T) location is at tile boundary,
// and we're (S) approaching it parallel to the tile edge.
// The choice at the vertex can be arbitrary,
//  +---+---+
//  |:::|:::|
//  +-S-+-T-+
//  |:::|   | <-- the step can end up in here, resulting U-turn path.
//  +---+---+
static int fixupShortcuts(dtPolyRef *path, int npath,
                          dtNavMeshQuery *navQuery) {
  if (npath < 3) return npath;

  // Get connected polygons
  static const int maxNeis = 16;
  dtPolyRef neis[maxNeis];
  int nneis = 0;

  const dtMeshTile *tile = 0;
  const dtPoly *poly = 0;
  if (dtStatusFailed(navQuery->getAttachedNavMesh()->getTileAndPolyByRef(
          path[0], &tile, &poly)))
    return npath;

  for (unsigned int k = poly->firstLink; k != DT_NULL_LINK;
       k = tile->links[k].next) {
    const dtLink *link = &tile->links[k];
    if (link->ref != 0) {
      if (nneis < maxNeis) neis[nneis++] = link->ref;
    }
  }

  // If any of the neighbour polygons is within the next few polygons
  // in the path, short cut to that polygon directly.
  static const int maxLookAhead = 6;
  int cut = 0;
  for (int i = dtMin(maxLookAhead, npath) - 1; i > 1 && cut == 0; i--) {
    for (int j = 0; j < nneis; j++) {
      if (path[i] == neis[j]) {
        cut = i;
        break;
      }
    }
  }
  if (cut > 1) {
    int offset = cut - 1;
    npath -= offset;
    for (int i = 1; i < npath; i++) path[i] = path[i + offset];
  }

  return npath;
}

enum SamplePolyAreas {
  SAMPLE_POLYAREA_GROUND,
  SAMPLE_POLYAREA_WATER,
  SAMPLE_POLYAREA_ROAD,
  SAMPLE_POLYAREA_DOOR,
  SAMPLE_POLYAREA_GRASS,
  SAMPLE_POLYAREA_JUMP
};

NavMesh::NavMesh() {
  m_straightPathOptions = 0;
  m_nstraightPath = 0;
  m_nsmoothPath = 0;

  m_navMesh = 0;
  m_navQuery = 0;
  m_toolMode = TOOLMODE_PATHFIND_SLICED;

  m_pathFindStatus = DT_FAILURE;
  m_pathIterNum = 0;

  m_navQuery = dtAllocNavMeshQuery();

  m_filter.setIncludeFlags(SAMPLE_POLYFLAGS_ALL ^ SAMPLE_POLYFLAGS_DISABLED);
  m_filter.setExcludeFlags(0);

  m_polyPickExt[0] = 2;
  m_polyPickExt[1] = 4;
  m_polyPickExt[2] = 2;

  m_filter.setAreaCost(SAMPLE_POLYAREA_GROUND, 1.0f);
  m_filter.setAreaCost(SAMPLE_POLYAREA_WATER, 10.0f);
  m_filter.setAreaCost(SAMPLE_POLYAREA_ROAD, 1.0f);
  m_filter.setAreaCost(SAMPLE_POLYAREA_DOOR, 1.0f);
  m_filter.setAreaCost(SAMPLE_POLYAREA_GRASS, 2.0f);
  m_filter.setAreaCost(SAMPLE_POLYAREA_JUMP, 1.5f);
}

NavMesh::~NavMesh() {
  dtFreeNavMesh(m_navMesh);
  m_navMesh = 0;
}

dtNavMesh *NavMesh::LoadSoloMeshFile(const char *file_name) {
  FILE *fp = fopen(file_name, "rb");
  if (!fp) return 0;

  // Read header.
  NavMeshSetHeader header;
  size_t readLen = fread(&header, sizeof(NavMeshSetHeader), 1, fp);
  if (readLen != 1) {
    fclose(fp);
    return 0;
  }

  if (header.magic != NAVMESHSET_MAGIC) {
    fclose(fp);
    return 0;
  }

  if (header.version != NAVMESHSET_VERSION) {
    fclose(fp);
    return 0;
  }

  dtNavMesh *mesh = dtAllocNavMesh();
  if (!mesh) {
    fclose(fp);
    return 0;
  }

  dtStatus status = mesh->init(&header.params);
  if (dtStatusFailed(status)) {
    fclose(fp);
    return 0;
  }

  // Read tiles.
  for (int i = 0; i < header.numTiles; ++i) {
    NavMeshTileHeader tileHeader;
    readLen = fread(&tileHeader, sizeof(tileHeader), 1, fp);
    if (readLen != 1) {
      fclose(fp);
      return 0;
    }

    if (!tileHeader.tileRef || !tileHeader.dataSize) break;

    unsigned char *data =
        (unsigned char *)dtAlloc(tileHeader.dataSize, DT_ALLOC_PERM);
    if (!data) break;
    memset(data, 0, tileHeader.dataSize);
    readLen = fread(data, tileHeader.dataSize, 1, fp);
    if (readLen != 1) {
      dtFree(data);
      fclose(fp);
      return 0;
    }

    mesh->addTile(data, tileHeader.dataSize, DT_TILE_FREE_DATA,
                  tileHeader.tileRef, 0);
  }

  fclose(fp);
  return mesh;
}

bool NavMesh::LoadSoloNavMesh(const char *file_name) {
  if (!file_name) {
    return false;
  }

  dtFreeNavMesh(m_navMesh);
  m_navMesh = LoadSoloMeshFile(file_name);
  if (!m_navMesh) {
    return false;
  }

  m_navQuery->init(m_navMesh, 65535 * 3);
  return true;
}

void NavMesh::RandomPoints(int count) {
  int test_count = count;
  std::cout << "test random point:" << test_count << std::endl;

  m_toolMode = TOOLMODE_PATHFIND_SLICED;
  int64_t start_now = time(0);

  for (int i = 0; i < test_count; i++) {
    float spos[3];
    float epos[3];

    dtPolyRef start_ref;
    dtPolyRef end_ref;

    dtStatus status =
        m_navQuery->findRandomPoint(&m_filter, frand, &start_ref, spos);
    if (dtStatusSucceed(status)) {
      status = m_navQuery->findRandomPoint(&m_filter, frand, &end_ref, epos);
      if (dtStatusSucceed(status)) {
        FindPath(spos, epos);
        assert(m_nstraightPath > 0);
        std::cout << "m_nstraightPath:" << m_nstraightPath << std::endl;
        for (int i = 0; i < m_nstraightPath; i++) {
          std::cout << "x:" << m_straightPath[i * 3]
                    << " y:" << m_straightPath[i * 3 + 1]
                    << " z:" << m_straightPath[i * 3 + 2] << std::endl;
        }
      }
    }
  }

  int64_t end_now = time(0);
  std::cout << "sliced cost time:" << end_now - start_now << std::endl;
}

void NavMesh::FindPath(float sp[3], float ep[3]) {
  dtVcopy(m_spos, sp);
  dtVcopy(m_epos, ep);

  m_navQuery->findNearestPoly(m_spos, m_polyPickExt, &m_filter, &m_startRef, 0);
  m_navQuery->findNearestPoly(m_epos, m_polyPickExt, &m_filter, &m_endRef, 0);

  m_pathFindStatus = DT_FAILURE;

  if (m_toolMode == TOOLMODE_PATHFIND_FOLLOW) {
    m_pathIterNum = 0;
    if (m_startRef && m_endRef) {
#ifdef DUMP_REQS
      printf("pi  %f %f %f  %f %f %f  0x%x 0x%x\n", m_spos[0], m_spos[1],
             m_spos[2], m_epos[0], m_epos[1], m_epos[2],
             m_filter.getIncludeFlags(), m_filter.getExcludeFlags());
#endif

      m_navQuery->findPath(m_startRef, m_endRef, m_spos, m_epos, &m_filter,
                           m_polys, &m_npolys, MAX_POLYS);

      m_nsmoothPath = 0;

      if (m_npolys) {
        // Iterate over the path to find smooth path on the detail mesh surface.
        dtPolyRef polys[MAX_POLYS];
        memcpy(polys, m_polys, sizeof(dtPolyRef) * m_npolys);
        int npolys = m_npolys;

        float iterPos[3], targetPos[3];
        m_navQuery->closestPointOnPoly(m_startRef, m_spos, iterPos, 0);
        m_navQuery->closestPointOnPoly(polys[npolys - 1], m_epos, targetPos, 0);

        static const float STEP_SIZE = 0.5f;
        static const float SLOP = 0.01f;

        m_nsmoothPath = 0;

        dtVcopy(&m_smoothPath[m_nsmoothPath * 3], iterPos);
        m_nsmoothPath++;

        // Move towards target a small advancement at a time until target
        // reached or when ran out of memory to store the path.
        while (npolys && m_nsmoothPath < MAX_SMOOTH) {
          // Find location to steer towards.
          float steerPos[3];
          unsigned char steerPosFlag;
          dtPolyRef steerPosRef;

          if (!getSteerTarget(m_navQuery, iterPos, targetPos, SLOP, polys,
                              npolys, steerPos, steerPosFlag, steerPosRef))
            break;

          bool endOfPath = (steerPosFlag & DT_STRAIGHTPATH_END) ? true : false;
          bool offMeshConnection =
              (steerPosFlag & DT_STRAIGHTPATH_OFFMESH_CONNECTION) ? true
                                                                  : false;

          // Find movement delta.
          float delta[3], len;
          dtVsub(delta, steerPos, iterPos);
          len = dtMathSqrtf(dtVdot(delta, delta));
          // If the steer target is end of path or off-mesh link, do not move
          // past the location.
          if ((endOfPath || offMeshConnection) && len < STEP_SIZE)
            len = 1;
          else
            len = STEP_SIZE / len;
          float moveTgt[3];
          dtVmad(moveTgt, iterPos, delta, len);

          // Move
          float result[3];
          dtPolyRef visited[16];
          int nvisited = 0;
          m_navQuery->moveAlongSurface(polys[0], iterPos, moveTgt, &m_filter,
                                       result, visited, &nvisited, 16);

          npolys = fixupCorridor(polys, npolys, MAX_POLYS, visited, nvisited);
          npolys = fixupShortcuts(polys, npolys, m_navQuery);

          float h = 0;
          m_navQuery->getPolyHeight(polys[0], result, &h);
          result[1] = h;
          dtVcopy(iterPos, result);

          // Handle end of path and off-mesh links when close enough.
          if (endOfPath && inRange(iterPos, steerPos, SLOP, 1.0f)) {
            // Reached end of path.
            dtVcopy(iterPos, targetPos);
            if (m_nsmoothPath < MAX_SMOOTH) {
              dtVcopy(&m_smoothPath[m_nsmoothPath * 3], iterPos);
              m_nsmoothPath++;
            }
            break;
          } else if (offMeshConnection &&
                     inRange(iterPos, steerPos, SLOP, 1.0f)) {
            // Reached off-mesh connection.
            float startPos[3], endPos[3];

            // Advance the path up to and over the off-mesh connection.
            dtPolyRef prevRef = 0, polyRef = polys[0];
            int npos = 0;
            while (npos < npolys && polyRef != steerPosRef) {
              prevRef = polyRef;
              polyRef = polys[npos];
              npos++;
            }
            for (int i = npos; i < npolys; ++i) polys[i - npos] = polys[i];
            npolys -= npos;

            // Handle the connection.
            dtStatus status = m_navMesh->getOffMeshConnectionPolyEndPoints(
                prevRef, polyRef, startPos, endPos);
            if (dtStatusSucceed(status)) {
              if (m_nsmoothPath < MAX_SMOOTH) {
                dtVcopy(&m_smoothPath[m_nsmoothPath * 3], startPos);
                m_nsmoothPath++;
                // Hack to make the dotted path not visible during off-mesh
                // connection.
                if (m_nsmoothPath & 1) {
                  dtVcopy(&m_smoothPath[m_nsmoothPath * 3], startPos);
                  m_nsmoothPath++;
                }
              }
              // Move position at the other side of the off-mesh link.
              dtVcopy(iterPos, endPos);
              float eh = 0.0f;
              m_navQuery->getPolyHeight(polys[0], iterPos, &eh);
              iterPos[1] = eh;
            }
          }

          // Store results.
          if (m_nsmoothPath < MAX_SMOOTH) {
            dtVcopy(&m_smoothPath[m_nsmoothPath * 3], iterPos);
            m_nsmoothPath++;
          }
        }
      }
    } else {
      m_npolys = 0;
      m_nsmoothPath = 0;
    }
  } else if (m_toolMode == TOOLMODE_PATHFIND_STRAIGHT) {
    if (m_startRef && m_endRef) {
#ifdef DUMP_REQS
      printf("ps  %f %f %f  %f %f %f  0x%x 0x%x\n", m_spos[0], m_spos[1],
             m_spos[2], m_epos[0], m_epos[1], m_epos[2],
             m_filter.getIncludeFlags(), m_filter.getExcludeFlags());
#endif
      m_navQuery->findPath(m_startRef, m_endRef, m_spos, m_epos, &m_filter,
                           m_polys, &m_npolys, MAX_POLYS);
      m_nstraightPath = 0;
      if (m_npolys) {
        // In case of partial path, make sure the end point is clamped to the
        // last polygon.
        float epos[3];
        dtVcopy(epos, m_epos);
        if (m_polys[m_npolys - 1] != m_endRef)
          m_navQuery->closestPointOnPoly(m_polys[m_npolys - 1], m_epos, epos,
                                         0);

        m_navQuery->findStraightPath(m_spos, epos, m_polys, m_npolys,
                                     m_straightPath, m_straightPathFlags,
                                     m_straightPathPolys, &m_nstraightPath,
                                     MAX_POLYS, m_straightPathOptions);
      }
    } else {
      m_npolys = 0;
      m_nstraightPath = 0;
    }
  } else if (m_toolMode == TOOLMODE_PATHFIND_SLICED) {
    if (m_startRef && m_endRef) {
#ifdef DUMP_REQS
      printf("ps  %f %f %f  %f %f %f  0x%x 0x%x\n", m_spos[0], m_spos[1],
             m_spos[2], m_epos[0], m_epos[1], m_epos[2],
             m_filter.getIncludeFlags(), m_filter.getExcludeFlags());
#endif
      m_npolys = 0;
      m_nstraightPath = 0;

      m_pathFindStatus =
          m_navQuery->initSlicedFindPath(m_startRef, m_endRef, m_spos, m_epos,
                                         &m_filter, DT_FINDPATH_ANY_ANGLE);

      while (true) {
        if (dtStatusInProgress(m_pathFindStatus)) {
          m_pathFindStatus = m_navQuery->updateSlicedFindPath(10, 0);
        }

        if (dtStatusSucceed(m_pathFindStatus)) {
          m_navQuery->finalizeSlicedFindPath(m_polys, &m_npolys, MAX_POLYS);
          m_nstraightPath = 0;
          if (m_npolys) {
            // In case of partial path, make sure the end point is clamped to
            // the last polygon.
            float epos[3];
            dtVcopy(epos, m_epos);
            if (m_polys[m_npolys - 1] != m_endRef)
              m_navQuery->closestPointOnPoly(m_polys[m_npolys - 1], m_epos,
                                             epos, 0);

            m_navQuery->findStraightPath(
                m_spos, epos, m_polys, m_npolys, m_straightPath,
                m_straightPathFlags, m_straightPathPolys, &m_nstraightPath,
                MAX_POLYS, DT_STRAIGHTPATH_AREA_CROSSINGS);
          }

          m_pathFindStatus = DT_FAILURE;

          if (m_nstraightPath == 0) {
            return;
          }

          m_pathIndex.clear();
          dtPolyRef startRef;
          float t = 0;

          for (int32_t i = 0; i < m_nstraightPath; ++i) {
            m_pathIndex.push_back(i);
          }

          auto it = m_pathIndex.begin();
          while (m_pathIndex.size() > 2 && it != m_pathIndex.end()) {
            auto startIndex = (*it);
            int32_t endIndex = 0;
            auto rit = m_pathIndex.rbegin();
            while (rit != m_pathIndex.rend() && (*rit) != (*it)) {
              endIndex = *rit;
              m_navQuery->findNearestPoly(m_spos, m_polyPickExt, &m_filter,
                                          &startRef, 0);
              m_navQuery->raycast(startRef, &m_straightPath[startIndex * 3],
                                  &m_straightPath[endIndex * 3], &m_filter, &t,
                                  m_hitNormal, m_polys, &m_npolys, MAX_POLYS);
              if (t > 1) {
                auto tempIt = it;
                ++tempIt;
                while (tempIt != m_pathIndex.end() && (*tempIt) != (*rit)) {
                  tempIt = m_pathIndex.erase(tempIt);
                }
                break;
              }

              ++rit;
            }
            ++it;
          }
          break;
        }
      }
    }
  } else {
    m_npolys = 0;
    m_nstraightPath = 0;
  }
}
